#define LOAD64_IS_ATOMIC

#include "Threading.h"
#include <stdio.h>
#include <time.h>

// you may beed to use -march=i686 to activate atomic intrinsics
// g++ -march=i686 -O3 -lpthread -lrt Threading.cpp -o Threading

// qatomic_i386.h
// http://computing.llnl.gov/tutorials/pthreads/
// http://gcc.gnu.org/onlinedocs/gcc/Atomic-Builtins.html

static bool compute_time_out(timespec *ts, int ms)
{
	if(clock_gettime(CLOCK_REALTIME, ts) != 0)
		return false;
	ts->tv_nsec += (ms % 1000) * 1000000;
	ts->tv_sec += ms / 1000;
	if(ts->tv_nsec >= 1000000000)
	{
		++ts->tv_sec;
		ts->tv_nsec -= 1000000000;
	}
	return true;
}

inline bool Mutex::lock(int timeout_ms)
{
	timespec ts;
	if(!compute_time_out(&ts, timeout_ms)) return false;
	bool ret = pthread_mutex_timedlock(&mMutex, &ts) == 0;
	//if(ret) pthread_cleanup_push(cleanup_mutex, &mMutex);
	return ret;
}

LockException::LockException(const char *message)
{
	mMessage = message;
}

const char *LockException::what() const throw()
{
	return mMessage;
}



#if !defined(NO_ATOMIC)

Thread::~Thread()
{
	join();
}

AutoResetEvent::AutoResetEvent(bool initialState)
	: mState(initialState ? 1 : 0), mWaiters(0)
{
	pthread_mutex_init(&mMutex, NULL);
	pthread_cond_init(&mCond, NULL);
}

AutoResetEvent::~AutoResetEvent()
{
	pthread_cond_destroy(&mCond);
	pthread_mutex_destroy(&mMutex);
}

bool AutoResetEvent::set()
{
	int waiters = mWaiters;

	if(waiters == 0)
	{
		mState.fetchAndAdd(1);
		waiters = mWaiters;

		if(waiters > 0)
			mState.addAndFetch(waiters - 1);
		else
			return true;
	}
	else
		mState.addAndFetch(waiters);

	return pthread_cond_broadcast(&mCond) == 0;
}

bool AutoResetEvent::wait()
{
	int ret = 0;

	mWaiters.fetchAndAdd(1);
	if(mState.fetchAndSub(1) <= 0)
	{
		if(pthread_mutex_lock(&mMutex) != 0) { mState.fetchAndAdd(1); mWaiters.fetchAndSub(1); return false; }
		do {
			mState.fetchAndAdd(1);
			// Note: Unwanted wakeups from pthread_cond_wait() may occur (since another thread could have obtained the mutex,
			// changed the state and released the mutex, prior to this thread obtaining the mutex).
			ret = pthread_cond_wait(&mCond, &mMutex);
			if(ret != 0) break;
		} while(mState.fetchAndSub(1) <= 0);
		pthread_mutex_unlock(&mMutex);
	}
	mWaiters.fetchAndSub(1);

	return ret == 0;
}

bool AutoResetEvent::wait(int timeout_ms)
{
	int ret = 0;

	mWaiters.fetchAndAdd(1);
	if(mState.fetchAndSub(1) <= 0)
	{
		timespec ts;
		if(!compute_time_out(&ts, timeout_ms) || pthread_mutex_lock(&mMutex) != 0) { mState.fetchAndAdd(1); mWaiters.fetchAndSub(1); return false; }
		do {
			mState.fetchAndAdd(1);
			// Note: Unwanted wakeups from pthread_cond_wait() may occur (since another thread could have obtained the mutex,
			// changed the state and released the mutex, prior to this thread obtaining the mutex).
			ret = pthread_cond_timedwait(&mCond, &mMutex, &ts);
			if(ret != 0) break;
		} while(mState.fetchAndSub(1) <= 0);
		pthread_mutex_unlock(&mMutex);
	}
	mWaiters.fetchAndSub(1);

	return ret == 0;
}

ManualResetEvent::ManualResetEvent(bool initialState)
	: mState(initialState ? 1 : 0), mWaiters(0)
{
	pthread_mutex_init(&mMutex, NULL);
	pthread_cond_init(&mCond, NULL);
}

ManualResetEvent::~ManualResetEvent()
{
	pthread_cond_destroy(&mCond);
	pthread_mutex_destroy(&mMutex);
}

bool ManualResetEvent::wait()
{
	int ret = 0;

	mWaiters.fetchAndAdd(1);
	if(mState == 0)
	{
		if(pthread_mutex_lock(&mMutex) != 0) { mWaiters.fetchAndSub(1); return false; }
		do {
			// Note: Unwanted wakeups from pthread_cond_wait() may occur (since another thread could have obtained the mutex,
			// changed the state and released the mutex, prior to this thread obtaining the mutex).
			ret = pthread_cond_wait(&mCond, &mMutex);
			if(ret != 0) break;
		} while(mState == 0);
		pthread_mutex_unlock(&mMutex);
	}
	mWaiters.fetchAndSub(1);

	return ret == 0;
}

bool ManualResetEvent::wait(int timeout_ms)
{
	int ret = 0;

	mWaiters.fetchAndAdd(1);
	if(mState == 0)
	{
		timespec ts;
		if(!compute_time_out(&ts, timeout_ms) || pthread_mutex_lock(&mMutex) != 0) { mWaiters.fetchAndSub(1); return false; }
		do {
			// Note: Unwanted wakeups from pthread_cond_wait() may occur (since another thread could have obtained the mutex,
			// changed the state and released the mutex, prior to this thread obtaining the mutex).
			ret = pthread_cond_timedwait(&mCond, &mMutex, &ts);
			if(ret != 0) break;
		} while(mState == 0);
		pthread_mutex_unlock(&mMutex);
	}
	mWaiters.fetchAndSub(1);

	return ret == 0;
}

bool Semaphore::take()
{
	mWaiting.fetchAndAdd(1);
	if(mIsClosed) return false;
	bool ret = sem_wait(&mSem) == 0;
	mWaiting.fetchAndSub(1);
	return ret && !mIsClosed;
}

bool Semaphore::take(int timeout_ms)
{
	mWaiting.fetchAndAdd(1);
	if(mIsClosed) return false;
	timespec ts;
	if(!compute_time_out(&ts, timeout_ms)) return false;
	bool ret = sem_timedwait(&mSem, &ts) == 0;
	mWaiting.fetchAndSub(1);
	return ret && !mIsClosed;
}

bool Semaphore::close()
{
	mIsClosed.fetchAndOr(1);
	while(mWaiting)
	{
		int ret;
		if(sem_getvalue(&mSem, &ret) != 0)
			return false;
		if(ret == 0) {
			if(sem_post(&mSem) != 0)
				return false;
		} else {
			Thread::yield();
		}
	}
}

#else // if !defined(NO_ATOMIC)

AutoResetEvent::AutoResetEvent(bool initialState)
	: mState(initialState ? 1 : 0), mWaiters(0)
{
	pthread_mutex_init(&mMutex, NULL);
	pthread_cond_init(&mCond, NULL);
}

AutoResetEvent::~AutoResetEvent()
{
	pthread_cond_destroy(&mCond);
	pthread_mutex_destroy(&mMutex);
}

bool AutoResetEvent::set()
{
	if(pthread_mutex_lock(&mMutex) != 0) return false;
	int ret, waiters = mWaiters;
	if(waiters)
	{
		mState = waiters;
		ret = pthread_cond_broadcast(&mCond);
	}
	else
	{
		mState = 1;
		ret = 0;
	}
	pthread_mutex_unlock(&mMutex);
	return ret == 0;
}

bool AutoResetEvent::wait()
{
	if(pthread_mutex_lock(&mMutex) != 0) return false;
	++mWaiters;
	int ret = 0, state;
	while((state = mState) == 0)
	{
		// Note: Unwanted wakeups from pthread_cond_wait() may occur (since another thread could have obtained the mutex,
		// changed the state and released the mutex, prior to this thread obtaining the mutex).
		ret = pthread_cond_wait(&mCond, &mMutex);
		if(ret != 0) break;
	}
	if(ret == 0) mState = state - 1;
	--mWaiters;
	pthread_mutex_unlock(&mMutex);
	return ret == 0;
}

bool AutoResetEvent::wait(int timeout_ms)
{
	timespec ts;
	if(!compute_time_out(&ts, timeout_ms)) return false;
	if(pthread_mutex_lock(&mMutex) != 0) return false;
	++mWaiters;
	int ret = 0, state;
	while((state = mState) == 0)
	{
		// Note: Unwanted wakeups from pthread_cond_wait() may occur (since another thread could have obtained the mutex,
		// changed the state and released the mutex, prior to this thread obtaining the mutex).
		ret = pthread_cond_timedwait(&mCond, &mMutex, &ts);
		if(ret != 0) break;
	}
	if(ret == 0) mState = state - 1;
	--mWaiters;
	pthread_mutex_unlock(&mMutex);
	return ret == 0;
}

ManualResetEvent::ManualResetEvent(bool initialState)
	: mState(initialState ? 1 : 0)
{
	pthread_mutex_init(&mMutex, NULL);
	pthread_cond_init(&mCond, NULL);
}

ManualResetEvent::~ManualResetEvent()
{
	pthread_cond_destroy(&mCond);
	pthread_mutex_destroy(&mMutex);
}

bool ManualResetEvent::set()
{
	if(pthread_mutex_lock(&mMutex) != 0) return false;
	mState = 1;
	int ret = pthread_cond_broadcast(&mCond);
	pthread_mutex_unlock(&mMutex);
	return ret == 0;
}

bool ManualResetEvent::reset()
{
	if(pthread_mutex_lock(&mMutex) != 0) return false;
	mState = 0;
	pthread_mutex_unlock(&mMutex);
}

bool ManualResetEvent::wait()
{
	if(pthread_mutex_lock(&mMutex) != 0) return false;
	int ret = 0;
	while(!mState)
	{
		// Note: Unwanted wakeups from pthread_cond_wait() may occur (since another thread could have obtained the mutex,
		// changed the state and released the mutex, prior to this thread obtaining the mutex).
		ret = pthread_cond_wait(&mCond, &mMutex);
		if(ret != 0) break;
	}
	pthread_mutex_unlock(&mMutex);
	return ret == 0;
}

bool ManualResetEvent::wait(int timeout_ms)
{
	timespec ts;
	if(!compute_time_out(&ts, timeout_ms)) return false;
	if(pthread_mutex_lock(&mMutex) != 0) return false;
	int ret = 0;
	while(!mState)
	{
		// Note: Unwanted wakeups from pthread_cond_wait() may occur (since another thread could have obtained the mutex,
		// changed the state and released the mutex, prior to this thread obtaining the mutex).
		ret = pthread_cond_timedwait(&mCond, &mMutex, &ts);
		if(ret != 0) break;
	}
	pthread_mutex_unlock(&mMutex);
	return ret == 0;
}

#endif // if !defined(NO_ATOMIC)


// TEST


const int N = 10 * 1024 * 1024;
FixedSizeQueue<int> q(N);
Atomic<int> got[N];

int main()
{
	class : public Thread
	{
	public:
		void *run()
		{
			int cnt = 0;
			for(;;)
			{
				int idx;
				if(q.dequeue(idx))
				{
					got[idx].fetchAndAdd(1);
					++cnt;
				}
				else
					break;
			}
			return 0;
		}
	} c1, c2;
	
	c1.start();
	c2.start();
	
	class Producer : public Thread
	{
	public:
		int mStart;
		Producer(int start) : mStart(start){}

		void *run()
		{
			for(int i = mStart ; i < N ; i += 2)
			{
				if(!q.enqueue(i))
					printf("enqueue(%i) failed\n", i);
			}
			printf("Done %i\n", mStart);
			return 0;
		}
	} p1(0), p2(1);

	p1.start();
	p2.start();

	p1.join();
	p2.join();

	printf("Done sending.\n");

	q.close();

	c1.join();
	c2.join();
	
	int cnt = 0;
	for(int i = 0 ; i < N ; ++i)
	{
		cnt += got[i];
		if(got[i] != 1) printf("missing %i\n", i);
	}
	printf("Sum: %i/%i\n", cnt, N);

	return 0;
}
